ME 305 - HW0x03
Authors Caleb Savard, Chris Linthacum
Created 2/17/2022
Modified 2/19/2022
This script uses some derivations from previous hand calculations.
First, the necessary symbolic variables are created.
clear
syms theta_bar_y...
theta_y...
theta_dot_y...
theta_ddot_y...
X...
X_dot...
X_ddot...
T_x...
...
r_g...
r_c...
r_g...
r_c...
r_b...
r_m...
l_p...
...
I_p...
I_b...
m_p...
m_b...
g
Next, various useful vectors are built.
g_bar = [0; 0; -g];
M_bar_y = [0; -l_p*T_x/r_m; 0]; % <-- This comes from a previous calc
theta_bar_y = [0; theta_y; 0];
theta_bar_dot_y = [0; theta_dot_y; 0];
theta_bar_ddot_y = [0; theta_ddot_y; 0];
r_bar_g_o = [r_g*sin(theta_y);0;r_g*cos(theta_y)];
r_bar_c_o = [r_c*sin(theta_y); 0; r_c*cos(theta_y)];
r_bar_b_c = [r_b*sin(theta_y); 0; r_b*cos(theta_y)];
r_bar_b_f = [X*cos(theta_y); 0; -X*sin(theta_y)];
r_bar_f_o = r_bar_b_c + r_bar_c_o;
r_bar_b_o = r_bar_f_o + r_bar_b_f;
r_bar_b_e = r_bar_b_c;
Velocity of the platform and the ball are calculated. The intermediate reference frame is placed above the plate, level with the ball's center.
v_bar_g = cross(theta_bar_y, r_bar_g_o);
v_bar_f = cross(theta_bar_dot_y, r_bar_f_o);
v_bar_rel = [X_dot*cos(theta_y); 0; -X_dot*sin(theta_y)];
v_bar_b = v_bar_f + v_bar_rel + cross(theta_bar_dot_y, r_bar_b_f);
Acceleration of the platform and the ball are calculated.
a_bar_p = cross(theta_bar_ddot_y, r_bar_g_o) + cross(theta_bar_dot_y,cross(theta_bar_dot_y, r_bar_g_o));
a_bar_f = cross(theta_bar_ddot_y, r_bar_f_o) + cross(theta_bar_dot_y,cross(theta_bar_dot_y, r_bar_f_o));
a_bar_rel = [X_ddot*cos(theta_y); 0; -X_ddot*sin(theta_y)];
a_bar_b = a_bar_f + cross(theta_bar_ddot_y, r_bar_b_f) + cross(theta_bar_dot_y,cross(theta_bar_dot_y, r_bar_b_f))...
+ cross(2*theta_bar_dot_y, v_bar_rel) + a_bar_rel;
Euler's Equation of Motion, both for the platform, and the ball.
Eul_p_left = cross((r_bar_f_o + r_bar_b_f), m_b*g_bar) + cross(r_bar_g_o, m_p*g_bar) + M_bar_y;
Eul_p_right = simplify(I_p*theta_bar_ddot_y + cross(r_bar_g_o, m_p*a_bar_p) + I_b*[0; X_ddot/r_b + theta_ddot_y; 0] + cross(r_bar_b_o, m_b*a_bar_b));
eq_p = Eul_p_left(2) == Eul_p_right(2);
Eul_b_left = cross(r_bar_b_e, g_bar*m_b);
Eul_b_right = simplify(I_b*[0; X_ddot/r_b + theta_ddot_y; 0] + cross(r_bar_b_e, m_b*a_bar_b));
eq_b = Eul_b_left(2) == Eul_b_right(2);
Convert equation to matrices.
[M, f] = equationsToMatrix([eq_p, eq_b], [X_ddot, theta_ddot_y]);
dX = [X_dot; theta_dot_y; M\f]; % <-- That thing's nasty, don't unsuppress if you want to sleep in the near future.
Create the jacobians for the state vector and the output vector. Later, these will be used to solve for the A and B matrices.
jac_x = jacobian(dX, [X, theta_y, X_dot, theta_dot_y]);
jac_u = jacobian(dX, T_x);
Input known constants.
g = 9.81; % [m/s^2] Gravity
r_m = 0.06; % [m] Radius of Lever Arm
l_r = 0.05; % [m] Length of the push rod
r_b = 0.0105; % [m] Radius of the ball
r_g = 0.042; % [m] Vertical distance from the u-joint to the platform CG
l_p = 0.110; % [m] Horizontal distance from the push-rod pivot to the U-joint
r_p = 0.0325; % [m] Vertical distance from the u-joint to the push-rod pivot
r_c = 0.050; % [m] Vertical distance from the u-joint to the platform surface
m_b = 0.03; % [kg] Mass of the ball
m_p = 0.400; % [kg] Mass of the platform
I_p = 1.88e-3; % [kg m^2] Moment of inertia of the platform (about a horizontal axis through CG)
%b = 10e-3; % [N m s/rad] Viscous friction in the u-joint
I_b = (2/5)*m_b*r_b^2;
Substitute known values and solve for an equalibrium point where the derivateve of the state vector is zero.
equilibrium = solve(subs(dX) == 0, [X, theta_y, X_dot, theta_dot_y, T_x]);
eq_vals = subs(equilibrium)
Assign these values to their symbolic variables and substitute them into the jacobian matrices for matrix A and B.
X = eq_vals.X;
X_dot = eq_vals.X_dot;
theta_y = eq_vals.theta_y;
theta_dot_y = eq_vals.theta_dot_y;
T_x = eq_vals.T_x;
A = double(subs(jac_x))
B = double(subs(jac_u))
C and D are simply the identity matrix and zeros since the state variables are all we care about.
C = eye(4)
D = zeros(4, 1)
Now that the A, B, C, and D matrices are finished, the system can be simulated. Recall that the state vector is
sys = ss(A, B, C, D);
t = linspace(0, 1, 20);
u = zeros(length(t), 1);
x0 = [0 0 0 0];
[sim1Data, tout1] = lsim(sys, u, t, x0);
figure(1)
subplot(2,2,1)
plot(tout1, sim1Data(:,1))
xlabel('Time [s]')
ylabel('Displacement [m]')
title('Ball Displacement X')
subplot(2,2,2)
plot(tout1, sim1Data(:,2))
xlabel('Time [s]')
ylabel('Angle [rad]')
title('Platform Angle $\theta_y$')
subplot(2,2,3)
plot(tout1, sim1Data(:,3))
xlabel('Time [s]')
ylabel('Velocity [m/s]')
title('Ball Velocity $\dot X$')
subplot(2,2,4)
plot(tout1, sim1Data(:,4))
xlabel('Time [s]')
ylabel('Angular Velocity [rad/s]')
title('Platform Angular Velocity $\dot \theta_y$')
sgtitle(figure(1), 'System Response where $X_0 = 0$')
This response, though trivial, does indicate the simulation is working properly. If the ball is placed at the center (X = 0), the platform is level, nothing is moving and no torque is applied, nothing should happen. This is, after all, the equilibrium point. Next, the system will be simulated for similar initial conditions except the ball is offset by 5 cm.
t = linspace(0, 0.4, 20);
x0 = [0.5 0 0 0];
[sim2Data, tout2] = lsim(sys, u, t, x0);
figure(2)
subplot(2,2,1)
plot(tout2, sim2Data(:,1))
xlabel('Time [s]')
ylabel('Displacement [m]')
title('Ball Displacement X')
subplot(2,2,2)
plot(tout2, sim2Data(:,2))
xlabel('Time [s]')
ylabel('Angle [rad]')
title('Platform Angle $\theta_y$')
subplot(2,2,3)
plot(tout2, sim2Data(:,3))
xlabel('Time [s]')
ylabel('Velocity [m/s]')
title('Ball Velocity $\dot X$')
subplot(2,2,4)
plot(tout2, sim2Data(:,4))
xlabel('Time [s]')
ylabel('Angular Velocity [rad/s]')
title('Platform Angular Velocity $\dot \theta_y$')
sgtitle(figure(2), 'System Response where $X_0 = 0.05$')
This also makes sense. The offset ball should cause the platform to tilt as seen in the plots on the right. The left-side plots show the ball briefly moving toward the center as the platform swings under it, before rolling toward the edge of the platform.